#include "manager.h"

#include "FreeRTOS.h"
#include "task.h"

#include "rc.h"
#include "robot.h"
#include "robot_state.h"

const RC_ctrl_t *local_rc_ctrl;//SBUS协议数据接收结构体

static void manager_task(void *parameter)
{
    TickType_t last_wake_time = xTaskGetTickCount();

    char zone_bit_1;
    char zone_bit_2;

    while(SL_TRUE)
    {
        if(zone_bit_1 != local_rc_ctrl->rc.s[0] | zone_bit_2 != local_rc_ctrl->rc.s[1] ) 
    {
        if(local_rc_ctrl->rc.s[0] == 0 && local_rc_ctrl->rc.s[1] == 0)
        {
            set_robot_state(ROBOT_NORM);
        }
        else if(local_rc_ctrl->rc.s[0] == 0 && local_rc_ctrl->rc.s[1] == 1)
        {
            set_robot_state(ROBOT_NORM);
        }
         else if(local_rc_ctrl->rc.s[0] == 0 && local_rc_ctrl->rc.s[1] == 2)
        {
            set_robot_state(ROBOT_NORM);
        }
        else if(local_rc_ctrl->rc.s[0] == 1 && local_rc_ctrl->rc.s[1] == 0)
        {
            set_robot_state(ROBOT_INIT);
        }        
        else if(local_rc_ctrl->rc.s[0] == 1 && local_rc_ctrl->rc.s[1] == 1)
        {
            set_robot_state(ROBOT_TOP);
        }    
        else if(local_rc_ctrl->rc.s[0] == 1 && local_rc_ctrl->rc.s[1] == 2)
        {
            set_robot_state(ROBOT_IDLE);
        }                 
        else
        {
            set_robot_state(ROBOT_DISABLE);
        }
            }

        zone_bit_1 = local_rc_ctrl->rc.s[0];
        zone_bit_2 = local_rc_ctrl->rc.s[1];   

        vTaskDelayUntil(&last_wake_time, ROBOT_TASK_PERIOD_MS);    
    }
}

void manger_init(void)
{
    remote_control_init();
    local_rc_ctrl = get_remote_control_point();

    xTaskCreate(manager_task, "manager", 512, NULL, 10, NULL);
}


